#include "motor.h"

TIM_HandleTypeDef* motor_htim;
uint32_t motor_channel_left_1;
uint32_t motor_channel_left_2;
uint32_t motor_channel_right_1;
uint32_t motor_channel_right_2;
int32_t motor_range_max = 0;

/**
  * @brief  Init motor 
  * @param	handle of tim, channels, max range(equal to ARR)
  * @retval None
  */
void motor_init(TIM_HandleTypeDef* htim, uint32_t channel_left_1, uint32_t channel_left_2, uint32_t channel_right_1, uint32_t channel_right_2, int32_t max)
{
	motor_htim = htim;
	motor_channel_left_1 = channel_left_1;
	motor_channel_left_2 = channel_left_2;
	motor_channel_right_1 = channel_right_1;
	motor_channel_right_2 = channel_right_2;
	motor_range_max = max + 1;
}

static float motor_limit(float speed)
{
	float result = 0;
	result = (speed > 1) ? 1 : speed;
	result = (speed <	-1) ? -1 : speed;
	return result;
}

/**
  * @brief  Set speed of motor
  * @param	Left motor speed and right motor speed [-1, 1].
  * @retval None
  */
void motor_set(float speed_left, float speed_right)
{
	if(speed_left < 0)
	{
		speed_left = - motor_limit(speed_left) * motor_range_max;
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_1, speed_left);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_2, 0);
	}
	else if(speed_left > 0)
	{
		speed_left = motor_limit(speed_left) * motor_range_max;
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_1, 0);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_2, speed_left);
		
	}
	else
	{
		speed_left = motor_limit(speed_left) * motor_range_max;
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_1, motor_range_max);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_left_2, motor_range_max);
		
	}
	
	if(speed_right < 0)
	{
		speed_right = - motor_limit(speed_right) * motor_range_max;
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_1, speed_right);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_2, 0);
	}
	else if(speed_right > 0)
	{
		speed_right = motor_limit(speed_right) * motor_range_max;
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_1, 0);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_2, speed_right);
	}
	else
	{
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_1, motor_range_max);
		__HAL_TIM_SetCompare(motor_htim, motor_channel_right_2, motor_range_max);
	}
}